#include <iostream>
#include <cmath>

#include "arenas/arena_jumping_ball.h"
#include "output_cart_ball.h"

using namespace std;

Output_cart_ball::Output_cart_ball()
{
}

Output_cart_ball::~Output_cart_ball()
{
}

double Output_cart_ball::get_output_value(void)
{
  double g = 9.82;
  double acc = 30;
  double pbx = *(_arena->get_ball_x_position_p());
  double pby = *(_arena->get_ball_y_position_p());
  double vbx = *(_arena->get_ball_x_velocity_p());
  double vby = *(_arena->get_ball_y_velocity_p());

  double x = *(_arena->get_position_p());
  double v = *(_arena->get_velocity_p());
  // get time to pby = 0
  double t0 = (vby + sqrt(vby * vby + 2.0 * g * pby)) / g;
  // get distance to touch down
  double x0 = t0 * vbx + pbx - x;

//  cout << "t0 = " << t0 << endl;
//  cout << "x0 = " << x0 << endl;

  if(x0>0 && v<0) {
    return acc;
  }
  if(x0<0 && v>0) {
    return -acc;
  }
  if (x0 > 0) {
    if (x0 > v * v / (1.0 * 2.0)) {
      return acc;
    }
    else {
      return -acc;
    }
  }
  else {
    if (fabs(x0) > v * v / (1.0 * 2.0)) {
      return -acc;
    }
    else {
      return acc;
    }
  }
}

void Output_cart_ball::set_arena(Arena_jumping_ball *arena)
{
  _arena = arena;
}

